from .PI_actor import PIActor
import numpy as np

class SlideMoldPIActor(PIActor):
    def __init__(self, goal_delta=0.2, max_df=0.7, kp=0.5, ki=1e-3, fg_ub=8.0, fg_lb=0.5):
        super().__init__(goal_delta, max_df, kp, ki, fg_ub, fg_lb)
        self.state["dft"] = np.zeros(2)  # Changing rate of tangential force
        self.state["ft"] = np.zeros(2)  # Tangential force
    
    def get_delta_fg(self, fg):
        """
        Calculates the change in grasp force based on the current delta and grasp force.
        
        Args:
            delta (float): The distance that current contact to macro slip.
            fg (float): The current grasp force at the contact point.
        
        Returns:
            float: The change in grasp force to apply.
        """
        delta = np.mean(self.state["delta"])
        ft = np.mean(self.state["ft"])
        dft = np.mean(self.state["dft"])

        ratio = 1 - delta
        self.integral_error += (ratio - self.goal_ratio) * fg
        self.integral_error = min(max(self.integral_error, -self.max_intergral_error), self.max_intergral_error)
        df = self.kp * (ratio - self.goal_ratio) * fg + self.ki * self.integral_error + dft / (ft + 1e-3) * fg
        df = min(max(df, -self.max_df), self.max_df)
        return df
    
    def restart(self):
        super().restart()
        self.state["dft"] = np.zeros(2)
        self.state["ft"] = np.zeros(2)
    
    def pack_state(self):
        superstate = super().pack_state()
        superstate["dft"] = self.state["dft"].mean()
        superstate["ft"] = self.state["ft"].mean()
        return superstate
